#pragma once
#include "processor.h"
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <eigen3/Eigen/Core>
#include <pcl/common/transforms.h>

#include <pcl/common/common.h>

#include <pcl/keypoints/harris_3d.h>

namespace CloudFunc{

template<typename PointType>
auto extractCloud(typename Processor<PointType>::CloudPtrType input,double dis_res,Eigen::Vector4d &norm){
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane_cloud (new pcl::PointCloud<pcl::PointXYZRGB>); 
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  // 创建内层点云索引指针
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // 创建点云分割对象
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // 设置点云分割优化参数：true
  seg.setOptimizeCoefficients (true);
  // 设置模型类型：平面
  seg.setModelType (pcl::SACMODEL_PLANE);
  // 设置筛选方法类型：RANSAC
  seg.setMethodType (pcl::SAC_RANSAC);
  // 设置距离阈值
  seg.setDistanceThreshold (dis_res);
  // 向分割器中填入点云
  seg.setInputCloud (input);
  // 开始分割（输出为指向内点的指针，分割出的平面模型系数）
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
  }
  // 输出平面模型系数
  std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3] << std::endl;
  // 输出分割后的内点
  norm(0) = coefficients->values[0];
  norm(1) = coefficients->values[1];
  norm(2) = coefficients->values[2];
  norm(3) = coefficients->values[3];
  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
  
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  std::cout<<1<<std::endl;
  extract.setInputCloud(input);
  std::cout<<2<<std::endl;
  extract.setIndices(inliers);
  extract.setNegative(false);
  extract.filter(*plane_cloud);  //平面
  std::cout<<3<<std::endl;
  extract.setNegative(true);
  std::cout<<4<<std::endl;
  extract.filter(*input);  //剩余
  std::cout<<5<<std::endl;
  return  plane_cloud;
}
double distaneToPanel(pcl::PointXYZRGB &p,Eigen::Vector4d &norm){
  double up   = std::abs(p.x*norm(0)+p.y*norm(1)+p.z*norm(2)+norm(3));
  double down = std::sqrt(norm(0)*norm(0)+norm(1)*norm(1)+norm(2)*norm(2));
  return up/down;
} 
auto get_plane_func = [](Processor<pcl::PointXYZRGB>::CloudPtrType input)->void {
  srand((unsigned)time(NULL));
  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloud_plane_vec;
  Eigen::Vector4d plane_norm0,plane_norm1,plane_norm2,plane_norm3,plane_norm4;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_all (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane_all (new pcl::PointCloud<pcl::PointXYZRGB>); 
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane = extractCloud<pcl::PointXYZRGB>(input,2,plane_norm0);

  uint8_t R = rand() % (256) + 0;
	uint8_t G = rand() % (256) + 0;
	uint8_t B = rand() % (256) + 0;
  for (int i=0;i< (cloud_plane->width * cloud_plane->height);i++)
  {
        pcl::PointXYZRGB point;
        point.x = cloud_plane->points[i].x;
        point.y = cloud_plane->points[i].y;
        point.z = cloud_plane->points[i].z;
        point.r = R;
        point.g = G;
        point.b = B;
        cloud_plane->points[i].r = R;
        cloud_plane->points[i].g = G;
        cloud_plane->points[i].b = B;
        cloud_plane_all->push_back(point);
  }
  cloud_plane_vec.push_back(cloud_plane);
  cloud_plane = extractCloud<pcl::PointXYZRGB>(input,3.0,plane_norm1);
  R = rand() % (256) + 0;
	G = rand() % (256) + 0;
	B = rand() % (256) + 0;
  for (int i=0;i< (cloud_plane->width * cloud_plane->height);i++)
  {
        pcl::PointXYZRGB point;
        point.x = cloud_plane->points[i].x;
        point.y = cloud_plane->points[i].y;
        point.z = cloud_plane->points[i].z;
        point.r = R;
        point.g = G;
        point.b = B;
        cloud_plane->points[i].r = R;
        cloud_plane->points[i].g = G;
        cloud_plane->points[i].b = B;
        cloud_plane_all->push_back(point);
  }
  cloud_plane_vec.push_back(cloud_plane);
  cloud_plane = extractCloud<pcl::PointXYZRGB>(input,2.5,plane_norm2);
  R = rand() % (256) + 0;
	G = rand() % (256) + 0;
	B = rand() % (256) + 0;
  for (int i=0;i< (cloud_plane->width * cloud_plane->height);i++)
  {
        pcl::PointXYZRGB point;
        point.x = cloud_plane->points[i].x;
        point.y = cloud_plane->points[i].y;
        point.z = cloud_plane->points[i].z;
        point.r = R;
        point.g = G;
        point.b = B;
        cloud_plane->points[i].r = R;
        cloud_plane->points[i].g = G;
        cloud_plane->points[i].b = B;
        cloud_plane_all->push_back(point);
  }
  cloud_plane_vec.push_back(cloud_plane);
  cloud_plane = extractCloud<pcl::PointXYZRGB>(input,2.5,plane_norm3);
  R = rand() % (256) + 0;
	G = rand() % (256) + 0;
	B = rand() % (256) + 0;
  for (int i=0;i< (cloud_plane->width * cloud_plane->height);i++)
  {
        pcl::PointXYZRGB point;
        point.x = cloud_plane->points[i].x;
        point.y = cloud_plane->points[i].y;
        point.z = cloud_plane->points[i].z;
        point.r = R;
        point.g = G;
        point.b = B;
        cloud_plane->points[i].r = R;
        cloud_plane->points[i].g = G;
        cloud_plane->points[i].b = B;
        cloud_plane_all->push_back(point);
  }
  cloud_plane_vec.push_back(cloud_plane);
  cloud_plane = extractCloud<pcl::PointXYZRGB>(input,4.0,plane_norm4);
  R = rand() % (256) + 0;
	G = rand() % (256) + 0;
	B = rand() % (256) + 0;
  for (int i=0;i< (cloud_plane->width * cloud_plane->height);i++)
  {
        pcl::PointXYZRGB point;
        point.x = cloud_plane->points[i].x;
        point.y = cloud_plane->points[i].y;
        point.z = cloud_plane->points[i].z;
        point.r = R;
        point.g = G;
        point.b = B;
        cloud_plane->points[i].r = R;
        cloud_plane->points[i].g = G;
        cloud_plane->points[i].b = B;
        cloud_plane_all->push_back(point);
  }
  cloud_plane_vec.push_back(cloud_plane);
  //Eigen::Vector4d plane_norm0,plane_norm1,plane_norm2,plane_norm3,plane_norm4;
  double d = 0.5;
  for(auto p : cloud_plane_vec[0]->points){
      double d1 = distaneToPanel(p,plane_norm1);
      double d2 = distaneToPanel(p,plane_norm2);
      double d3 = distaneToPanel(p,plane_norm3);
      double d4 = distaneToPanel(p,plane_norm4);
      
      if(d1 < d || d2 <d || d3 <d || d4 <d){
        p.r = 255;
        p.g = 0;
        p.b = 0;
        result_all->push_back(p);
      }
      //std::cout<<d1<<std::endl;
  }
  for(auto p : cloud_plane_vec[1]->points){
      //double d0 = distaneToPanel(p,plane_norm1); //到自己平面
      double d1 = distaneToPanel(p,plane_norm0);
      double d2 = distaneToPanel(p,plane_norm2);
      double d3 = distaneToPanel(p,plane_norm3);
      double d4 = distaneToPanel(p,plane_norm4);
      if((d1 < d || d2 <d|| d3 <d|| d4 <d)){
        p.r = 255;
        p.g = 0;
        p.b = 0;
        result_all->push_back(p);
        //std::cout<<d0<<std::endl;
      }
      //std::cout<<d1<<std::endl;
  }
  for(auto p : cloud_plane_vec[2]->points){
      double d1 = distaneToPanel(p,plane_norm0);
      double d2 = distaneToPanel(p,plane_norm1);
      double d3 = distaneToPanel(p,plane_norm3);
      double d4 = distaneToPanel(p,plane_norm4);
      if(d1 < d || d2 <d|| d3 <d|| d4 <d){
        p.r = 255;
        p.g = 0;
        p.b = 0;
        result_all->push_back(p);
      }
      //std::cout<<d1<<std::endl;
  }
  pcl::copyPointCloud(*result_all, *input);
};

pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractCloudLine(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,double dis_res,Eigen::Vector3d &line_point,Eigen::Vector3d &line_t){
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_line_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);//创建一个模型参数对象，用于记录结果
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);  //inliers表示误差能容忍的点 记录的是点云的序号
	pcl::ExtractIndices<pcl::PointXYZRGB> extract;//按照点云提取器

	pcl::SACSegmentation<pcl::PointXYZRGB> seg;     // 创建一个分割器
	seg.setOptimizeCoefficients(true);      // Optional，这个设置可以选定结果平面展示的点是分割掉的点还是分割剩下的点。
	seg.setModelType(pcl::SACMODEL_LINE);  // Mandatory-设置目标几何形状
	seg.setMethodType(pcl::SAC_RANSAC);     //分割方法：随机采样法
	seg.setDistanceThreshold(dis_res);         //设置误差容忍范围，也就是阈值 这东西不能太小，自己多试试
	int i=0,nr_points = input->size();//  分割前，边界点的原始点云的数量
	seg.setInputCloud(input);               //输入点云
	seg.segment(*inliers, *coefficients);   //分割点云，直线和直线的点法式参数
  std::cout<<"line vector begin "<<std::endl;
  std::cout<<"line vector end "<<std::endl;   
  std::cout<<coefficients->values.size();
  line_point(0) = coefficients->values[0];
  line_point(1) = coefficients->values[1];
  line_point(2) = coefficients->values[2];
  line_t(0) = coefficients->values[3];
  line_t(1) = coefficients->values[4];
  line_t(2) = coefficients->values[5];
  std::cout<<"line vector ok"<<std::endl;
	extract.setInputCloud(input);//待提取点云
	extract.setIndices(inliers);//平面点云对应的索引
	extract.setNegative(false);
	extract.filter(*cloud_line);//输出对应点云到cloud_line容器

  extract.setNegative(true);
  extract.filter(*input);  //剩余

  uint8_t R = rand() % (256) + 0;
	uint8_t G = rand() % (256) + 0;
	uint8_t B = rand() % (256) + 0;
	for (int i = 0; i < (cloud_line->width * cloud_line->height); i++)
	{
			pcl::PointXYZRGB point;
			point.x = cloud_line->points[i].x;
			point.y = cloud_line->points[i].y;
			point.z = cloud_line->points[i].z;
			point.r = R;
			point.g = G;
			point.b = B;
			cloud_line_rgb->push_back(point);
	}
  std::cout <<  "before return ";
  return cloud_line_rgb;
}
auto get_line_func = [](Processor<pcl::PointXYZRGB>::CloudPtrType input)-> void {
    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloud_line_vec;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_line_all(new pcl::PointCloud<pcl::PointXYZRGB>);
    //线上一点
    Eigen::Vector3d line_point0,line_point1,line_point2,line_point3,line_point4,line_point5,line_point6,line_point7,line_point8;
    //线方向向量
    Eigen::Vector3d line_t0,line_t1,line_t2,line_t3,line_t4,line_t5,line_t6,line_t7,line_t8;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr line = extractCloudLine(input,1.0,line_point0,line_t0);
    cloud_line_vec.push_back(line);
    double res = 2.5;
    line = extractCloudLine(input,res,line_point1,line_t1);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point2,line_t2);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point3,line_t3);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point4,line_t4);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point5,line_t5);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point6,line_t6);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point7,line_t7);
    cloud_line_vec.push_back(line);
    line = extractCloudLine(input,res,line_point8,line_t8);
    cloud_line_vec.push_back(line);
    for(auto cloud :cloud_line_vec){
        *result_line_all += *cloud;
    }
    input->clear();
    pcl::copyPointCloud(*result_line_all, *input);
};
}